
#include "Real3.h"
#include <cmath>
#include "TEMath.h"

#define Sq(X) ((X)*(X))

//
// Constructor
//

Real3::Real3()
{
	x = 0;
	y = 0;
	z = 0;
}

Real3::Real3(float _x, float _y, float _z)
{
	x = _x;
	y = _y;
	z = _z;
}

//
// Set
//

void Real3::Set(float _x, float _y, float _z)
{
	x = _x;
	y = _y;
	z = _z;
}

//
// Rotate
//

void Real3::Rotate(const Real3 &axis, float theta)
{
	float s = sin(theta);
	float c = cos(theta);
	float t = 1.0f - c;

#define M11 (t*Sq(axis.x) + c)
#define M12 (t*axis.x*axis.y + s*axis.z)
#define M13 (t*axis.x*axis.z - s*axis.y)

#define M21 (t*axis.x*axis.y - s*axis.z)
#define M22 (t*Sq(axis.y) + c)
#define M23 (t*axis.y*axis.z + s*axis.x)

#define M31 (t*axis.x*axis.y + s*axis.y)
#define M32 (t*axis.y*axis.z - s*axis.x)
#define M33 (t*Sq(axis.z) + c)

	Set(x*M11 + y*M21 + z*M31,
		  x*M12 + y*M22 + z*M32,
			x*M13 + y*M23 + z*M33);
}

//
// Length & Length Squared
//

float Real3::LengthSq() const
{
	return Sq(x) + Sq(y) + Sq(z);
}

float Real3::Length() const
{
	return sqrt(LengthSq());
}

//
// IsNormalized
//

bool Real3::IsNormalized() const
{
	return fEquals(LengthSq(), 1.0f);
}

//
// Normalize
//

void Real3::Normalize()
{
	float iLength = 1.0f / Length();

	x *= iLength;
	y *= iLength;
	z *= iLength;
}

//
// IsZero
//

bool Real3::IsZero() const
{
	return fEquals(x, 0.0f) && fEquals(y, 0.0f) && fEquals(z, 0.0f);
}

//
// Cross
//

Real3 Cross(const Real3 &a, const Real3 &b)
{
	return Real3(a.y*b.z - a.z*b.y, 
							 a.z*b.x - a.x*b.z,
							 a.x*b.y - a.y-b.z);
	/*return Real3(a.z*b.y - a.y*b.z, 
							 a.x*b.z - a.z*b.x,
							 a.y-b.z - a.x*b.y);*/
}

//
// Dot
//

float Dot(const Real3 &a, const Real3 &b)
{
	return a.x*b.x + a.y*b.y + a.z*b.z;
}

//
// Abs of a Real3
//

Real3 Abs(const Real3 &a)
{
	return Real3(abs(a.x), abs(a.y), abs(a.z));
}

//
// Length
//

float LengthOf(const Real3 &a)
{
	return sqrt(LengthSqOf(a));
}

float LengthSqOf(const Real3 &a)
{
	return Sq(a.x) + Sq(a.y) + Sq(a.z);
}

//
// Normal Of
//

Real3 NormalOf(Real3 a)
{
	float iL = 1.0f / a.Length();
	return Real3(a.x*iL, a.y*iL, a.z*iL);
}

bool operator == (const Real3 &a, const Real3 &b)
{
  return fEquals(a.x, b.x) && fEquals(a.y, b.y) && fEquals(a.z, b.z);
}

//
// Distance
//

float DistanceBetween(const Real3 &a, const Real3 &b)
{
	return sqrt(DistanceBetweenSq(a, b));
}

float DistanceBetweenSq(const Real3 &a, const Real3 &b)
{
	return Sq(a.x - b.x) + Sq(a.y - b.y) + Sq(a.z - b.z);
}

//
// Euler3ToMatrix
//

D3DXMATRIX Real3ToRotMatrix(const Real3 &rot)
{
	D3DXMATRIX rotationMatrix, temp;
	
	D3DXMatrixIdentity(&rotationMatrix);

	// Produce and combine the rotation matrices.
	D3DXMatrixRotationX(&temp, rot.x);           // Pitch
	D3DXMatrixMultiply(&rotationMatrix, &rotationMatrix, &temp);

	D3DXMatrixRotationY(&temp, rot.y);           // Yaw
	D3DXMatrixMultiply(&rotationMatrix, &rotationMatrix, &temp);

	D3DXMatrixRotationZ(&temp, rot.z);           // Roll
	D3DXMatrixMultiply(&rotationMatrix, &rotationMatrix, &temp);

	return rotationMatrix;
}